#ifndef __MPU6050_LIGHT_DMP_H
#define __MPU6050_LIGHT_DMP_H

#include "mpu6050.h"
#include <math.h>

/****************************************************************************************************************/
/*                                        轻量级DMP配置                                                        */
/****************************************************************************************************************/

// 算法参数
#define MADGWICK_BETA           0.1f        // 算法增益参数，越大收敛越快但噪声越大
#define DMP_SAMPLE_RATE_HZ      100         // DMP采样率，与传感器采样率匹配
#define DMP_SAMPLE_PERIOD       (1.0f / DMP_SAMPLE_RATE_HZ)

// 传感器标定参数（需要根据实际传感器调整）
#define GYRO_SENSITIVITY_2000DPS    16.4f   // 陀螺仪灵敏度 LSB/(°/s)
#define ACCEL_SENSITIVITY_2G        16384.0f // 加速度计灵敏度 LSB/g

// 角度单位转换
#define DEG_TO_RAD              (M_PI / 180.0f)
#define RAD_TO_DEG              (180.0f / M_PI)

/****************************************************************************************************************/
/*                                        数据结构定义                                                         */
/****************************************************************************************************************/

// 轻量级DMP状态结构
typedef struct {
    // 四元数 (w, x, y, z)
    float q0, q1, q2, q3;
    
    // 欧拉角 (度)
    float roll;     // 横滚角 [-180, 180]
    float pitch;    // 俯仰角 [-90, 90]  
    float yaw;      // 偏航角 [-180, 180]
    
    // 角速度 (度/秒)
    float gyro_x, gyro_y, gyro_z;
    
    // 加速度 (g)
    float accel_x, accel_y, accel_z;
    
    // 状态标志
    bool initialized;
    uint32_t update_count;
} MPU6050_LightDMP_t;

// DMP配置结构
typedef struct {
    float beta;             // Madgwick算法参数
    uint16_t sample_rate;   // 采样率
    bool auto_calibrate;    // 是否自动校准
    uint16_t calibrate_samples; // 校准采样数
    bool use_magnetometer;  // 是否使用磁力计数据
} MPU6050_DMP_Config_t;

// 校准数据结构
typedef struct {
    float gyro_offset_x, gyro_offset_y, gyro_offset_z;
    float accel_offset_x, accel_offset_y, accel_offset_z;
    bool calibrated;
} MPU6050_Calibration_t;

/****************************************************************************************************************/
/*                                        函数声明                                                             */
/****************************************************************************************************************/

// 初始化和配置
uint8_t MPU6050_LightDMP_Init(MPU6050_DMP_Config_t *config);
uint8_t MPU6050_LightDMP_Calibrate(uint16_t samples);
uint8_t MPU6050_LightDMP_SetConfig(MPU6050_DMP_Config_t *config);

// 数据更新和获取
uint8_t MPU6050_LightDMP_Update(void);
uint8_t MPU6050_LightDMP_GetAttitude(float *roll, float *pitch, float *yaw);
uint8_t MPU6050_LightDMP_GetQuaternion(float *q0, float *q1, float *q2, float *q3);
uint8_t MPU6050_LightDMP_GetData(MPU6050_LightDMP_t *dmp_data);

// 工具函数
uint8_t MPU6050_LightDMP_Reset(void);
uint8_t MPU6050_LightDMP_GetStatus(bool *initialized, uint32_t *update_count);

// 低级算法函数（可选导出用于高级用户）
void Madgwick_Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, bool use_mag);
void Quaternion_To_Euler(float q0, float q1, float q2, float q3, float *roll, float *pitch, float *yaw);
float Fast_InvSqrt(float x);

#endif /* __MPU6050_LIGHT_DMP_H */ 